#include <stdio.h>
#include <unistd.h>
#include "ohos_init.h"
#include "cmsis_os2.h"

#include "bsp_encoder.h"
static void *MOTOR_Task(void)
{
    printf("motor start  \n");
    encoder_init();
    motor_init();

    int Encoder_All_Now[MAX_MOTOR] = {0};
    // Motor_Set_Pwm(0, 2); // 2500
    // Motor_Set_Pwm(1, 2);
    // Motor_Set_Pwm(2, 2);
    // Motor_Set_Pwm(3, 2);
    while (1)
    {
        osDelay(1000);
        Encoder_Update_Count();
        Encoder_Get_ALL(Encoder_All_Now);
        // M1 M2 M3 M4
        printf("1:%d,2:%d,3:%d,4:%d  \n", Encoder_All_Now[0], Encoder_All_Now[1], Encoder_All_Now[2], Encoder_All_Now[3]);
    }
}
static void ExampleEntry(void)
{

    osThreadAttr_t attr;

    attr.name = "MOTOR_Task";
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_mem = NULL;
    attr.stack_size = 2048;
    attr.priority = osPriorityNormal;

    if (osThreadNew((osThreadFunc_t)MOTOR_Task, NULL, &attr) == NULL)
    {
        printf("[ExampleEntry] create MOTOR_Task NG\n");
    }
    else
    {
        printf("[ExampleEntry] create MOTOR_Task OK\n");
    }
}
SYS_RUN(ExampleEntry);
